// Decompiled by Jad v1.5.8g. Copyright 2001 Pavel Kouznetsov.
// Jad home page: http://www.kpdus.com/jad.html
// Decompiler options: packimports(3) definits fieldsfirst ansi space safe 
// Source File Name:   WallFollow.java

package jones.behavior;

import jones.SimpleRobotInterface;

// Referenced classes of package jones.behavior:
//            TimeOut, Behavior, Arbiter

public class WallFollow
    implements Behavior
{
    private static class State
    {

        private final String name;
        public static final State START = new State("START");
        public static final State LOST_LEFT = new State("LOST_LEFT");
        public static final State LOST_RIGHT = new State("LOST_RIGHT");
        public static final State LEFT = new State("LEFT");
        public static final State RIGHT = new State("RIGHT");

        public String toString()
        {
            return name;
        }


        private State(String name)
        {
            this.name = name;
        }
    }


    private static final String NAME = "Wall Follow";
    private State state = null;
    private TimeOut timeOut = null;
    private double turnSpeed = 0;
    private double lostSpeed = 0;
    private double foundSpeed = 0;
    private double giveUpTime = 0;
    private boolean isOriented = false;

    public WallFollow()
    {
        state = State.START;
        timeOut = new TimeOut();
        turnSpeed = 100D;
        lostSpeed = 255D;
        foundSpeed = 50D;
        giveUpTime = 1.0D;
        isOriented = false;
    }

    public WallFollow(double turnSpeed, double lostSpeed, double foundSpeed, double giveUpTime)
    {
        state = State.START;
        timeOut = new TimeOut();
        this.turnSpeed = 100D;
        this.lostSpeed = 255D;
        this.foundSpeed = 50D;
        this.giveUpTime = 1.0D;
        isOriented = false;
        setTurnSpeed(turnSpeed);
        setLostSpeed(lostSpeed);
        setFoundSpeed(foundSpeed);
        setGiveUpTime(giveUpTime);
    }

    public String getName()
    {
        return "Wall Follow";
    }

    public void update(double d)
    {
    }

    public void setTurnSpeed(double turnSpeed)
    {
        this.turnSpeed = turnSpeed;
    }

    public double getTurnSpeed()
    {
        return turnSpeed;
    }

    public void setLostSpeed(double lostSpeed)
    {
        this.lostSpeed = lostSpeed;
    }

    public double getLostSpeed()
    {
        return lostSpeed;
    }

    public void setFoundSpeed(double foundSpeed)
    {
        this.foundSpeed = foundSpeed;
    }

    public double getFoundSpeed()
    {
        return foundSpeed;
    }

    public void setGiveUpTime(double giveUpTime)
    {
        this.giveUpTime = giveUpTime;
    }

    public double getGiveUpTime()
    {
        return giveUpTime;
    }

    public void act(Arbiter arbiter)
    {
        SimpleRobotInterface robot = (SimpleRobotInterface)arbiter.getRobotInterface(((Behavior) (this)));
        if (robot.getLastChosenBehavior() != this)
            state = State.START;
        if (state == State.START)
        {
            if (robot.isLeftIROn())
            {
                forwardRight(robot);
                state = State.LEFT;
            } else
            if (robot.isRightIROn())
            {
                forwardLeft(robot);
                state = State.RIGHT;
            } else
            {
                state = State.START;
            }
        } else
        if (state == State.LEFT)
        {
            forwardRight(robot);
            if (robot.isLeftIROn())
            {
                state = State.LEFT;
            } else
            {
                state = State.LOST_LEFT;
                timeOut.start();
            }
        } else
        if (state == State.RIGHT)
        {
            forwardLeft(robot);
            if (robot.isRightIROn())
            {
                state = State.RIGHT;
            } else
            {
                state = State.LOST_RIGHT;
                timeOut.start();
            }
        } else
        if (state == State.LOST_LEFT)
        {
            forwardLeft(robot);
            if (robot.isLeftIROn())
                state = State.LEFT;
            else
            if (timeOut.isPast(giveUpTime))
                state = State.START;
            else
                state = State.LOST_LEFT;
        } else
        if (state == State.LOST_RIGHT)
        {
            forwardRight(robot);
            if (robot.isRightIROn())
                state = State.RIGHT;
            else
            if (timeOut.isPast(giveUpTime))
                state = State.START;
            else
                state = State.LOST_RIGHT;
        } else
        {
            throw new NullPointerException("Null follow state");
        }
    }

    private void forwardLeft(SimpleRobotInterface robot)
    {
        if (isOriented)
        {
            forward(robot);
            isOriented = false;
        } else
        {
            robot.setLeftWheelSpeed((int)(-getTurnSpeed()));
            robot.setRightWheelSpeed((int)getTurnSpeed());
            isOriented = true;
        }
    }

    private void forwardRight(SimpleRobotInterface robot)
    {
        if (isOriented)
        {
            forward(robot);
            isOriented = false;
        } else
        {
            robot.setLeftWheelSpeed((int)getTurnSpeed());
            robot.setRightWheelSpeed((int)(-getTurnSpeed()));
            isOriented = true;
        }
    }

    private void forward(SimpleRobotInterface robot)
    {
        if (state == State.LEFT || state == State.RIGHT)
        {
            robot.setLeftWheelSpeed((int)getFoundSpeed());
            robot.setRightWheelSpeed((int)getFoundSpeed());
        } else
        if (state == State.LOST_LEFT || state == State.LOST_RIGHT)
        {
            robot.setLeftWheelSpeed((int)getLostSpeed());
            robot.setRightWheelSpeed((int)getLostSpeed());
        }
    }

    public String toString()
    {
        return "Wall Follow: " + state;
    }
}
